# -*- coding: utf-8 -*-
import math
import socket
import struct
from threading import Thread
import time
import config
import os
import json
import logging
import socket
import sqlite3
import subprocess
import modbus_tk.modbus_tcp as modbus_tcp
from modbus_tk.modbus_rtu import RtuServer
import modbus_tk.defines as cst
import time
import math
from collections import namedtuple
from periphery import GPIO
from modbus_tk.modbus_rtu import RtuMaster
from modbus_tk.defines import WRITE_MULTIPLE_REGISTERS,WRITE_SINGLE_COIL,WRITE_MULTIPLE_COILS,WRITE_SINGLE_REGISTER
from modbus_tk.defines import READ_COILS,READ_INPUT_REGISTERS,READ_HOLDING_REGISTERS,READ_DISCRETE_INPUTS
import struct
from serial import Serial
from serial.tools import list_ports
from threading import Thread
import threading

'''
这是新的二代版本，没有扩展板，Slave_driver为485模式。
修改时间：2023.11.11

增加FO1 为开塔机器人的第七轴控制， 2023.12.08

'''

robot_logger=logging.getLogger("Controler-Driver")
robot_logger.setLevel(logging.DEBUG)
lock_object = threading.Lock() # 创建一个线程锁 这个锁使用需谨慎

# 需要有三个进程：
# 一个是一直更新数据库状态
# 一个是根据数据库检测命令进行操作。
# 一个是一直检测机器人状态

algorithm_path = "home/cat/gm/algorithm_run/"

def delay_us(us):
    try:
        start = time.time()
        end =time.time()
        while (end-start)*1000000<us:
            end =time.time()

    except Exception as e:
        robot_logger.error(e,exc_info=True)


class Controler_Server():
    def __init__(self, host = "0.0.0.0", port = 502):
        super().__init__()
        # 获取本机的IP地址和名字
        host_name = socket.gethostname()
        self.host_add = socket.gethostbyname(host_name)


        self.host = host
        self.port = port

        self.server_tcp_init()

        self.robot = robot_driver()
        self.master = master_io()
        # self.board = Slaves_Driver()

        self.th_update = Thread(target= self.status_update)
        self.th_update.start()
        
        self.algorithm_th = Thread(target= self.run_algorithm)
        self.algorithm_th.start()
        
        self.tcpserver = TcpServer()
        self.algo_th = Thread(target= self.tcpserver.run)
        self.algo_th.start()      

    def server_tcp_init(self,):
        try:
            # if self.check_port_conflict():
            #     print(f"Port {self.port} has been used!!!")
            #     return False
            self.server = modbus_tcp.TcpServer(port=self.port,address=self.host,
                                               timeout_in_sec=5)
            self.server.start()
            self.register_init()
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def check_port_conflict(self,):
        try:
            ret = False
            sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            result = sock.connect_ex(self.host,self.port) # 检测有没有服务将这个端口以及IP地址占用
            if result == 0:
                ret = True
            else:
                ret = False
            sock.close()
            return ret
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def register_init(self,):

        self.slave =self.server.add_slave(1)  # 服务器上创建1号从机
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS,0,400) # 创建250个保持寄存器
        self.slave.add_block("fifo",cst.HOLDING_REGISTERS,1024,100) # 创建50个保持寄存器用于存储FIFO
        self.slave.add_block("motor",cst.HOLDING_REGISTERS,10000,50) # 创建50个保持寄存器用于存储电机状态
        self.slave.add_block("di",cst.COILS,1024,50) # 创建50个线圈寄存器用于存储di
        self.slave.add_block("do",cst.COILS,20000,50) # 创建50个线圈寄存器用于存储d0
        # self.slave.set_values("robot6",0,[1,2,3,4,5])
        # reg1 = self.slave.get_values("robot6",1,10)

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    def run_algorithm(self,):
            
        while True:
            try:

                time.sleep(0.5)
                num =   self.slave.get_values("robot6",181,1)[0]
                if num !=0:
                    match num:
                        case 1:
                            algorithm_name = "circle.py"

                        case 2:
                            algorithm_name = "color_lab.py"           
                        case 3:
                            algorithm_name = "shape.py"
                        case 4:
                            algorithm_name = "yolov8_detect.py"

                        case 5:
                            algorithm_name = "algorithm5.py"

                        case 6:
                            algorithm_name = "algorithm6.py"           
                        case 7:
                            algorithm_name = "algorithm7.py"
                        case 8:
                            algorithm_name = "algorithm8.py"
                        case _:
                            algorithm_name = "circle.py"
                    file_path = algorithm_path + algorithm_name

                    self.algo_th  = subprocess.Popen(["python3",file_path])   
                    time.sleep(2)
                    while self.algo_th.poll() is None:
                        time.sleep(0.5)
                    self.slave.set_values("robot6",181,0)
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)

                # 更新主机DI的状态
                m_di = []
                for i in range (0,13):       # 共有13个DI
                    if self.master.get_DI(num=i+1) == True:
                        m_di.append(0)
                    else:
                        m_di.append(1)
                self.slave.set_values("robot6",70,m_di)   # 获取DI并且刷新到MODBUS表中
                time.sleep(0.005)

                # 更新机器人的状态
                status = []
                slider_status = []
                s = self.robot.status_global
                if s != None:
                    match s["state"]:
                        case "Idle":
                            status = status + [1]
                        case "Alarm":
                            status = status + [2]
                        case "Home":
                            status = status + [3]
                        case "Run":
                            status = status + [4]
                        case _:
                            status = status + [0]
                    ang = s["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])

                    ang = s["coordinate"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])
                        if i ==6:
                            slider_status.append(a[0])
                            slider_status.append(a[1])
                    
                    status.append(self.robot.robot_type)
                    self.slave.set_values("robot6",0,status)
                    self.slave.set_values("motor",10000,slider_status)# 这里是使用modbus进行通讯IOA 


            except Exception as e:
                robot_logger.error(e,exc_info=True)

    # 启动在线线程
    def online(self):
        file_path = '/home/cat/gm/controler/process_dis.py'
        try:
            if os.path.exists(file_path):
                self.online_p = subprocess.Popen(f'python3 {file_path}',shell=True,preexec_fn=os.setsid)
            else:
                raise Exception(f"No {file_path}")
        except Exception as e:
            print(e)
        # 启动在线线程

    def dorun(self,):
        self.online()
        Mdo_flag = [0,0,0,0,0]
        bo = 0x00
        while True:
            try:
                time.sleep(0.1)

                # 1轮询执行DI的状态
                Mdo_list = self.slave.get_values("robot6",86,8)

                for i in range(5):   # 两个电机口和三个LED灯
                    if Mdo_list[i] == Mdo_flag[i]:
                        pass
                    else:
                        Mdo_flag[i]= Mdo_list[i]
                        match Mdo_list[i]:
                            case 0:
                                self.master.set_LED(num=i,value=False)  # DO输出低电平
                            case 1:
                                self.master.set_LED(num=i,value=True)   # DO输出高电平


                # 2执行DO的值
                if Mdo_list[7] == 1:
                    self.master.set_DO(num=Mdo_list[5],value=Mdo_list[6])
                    self.slave.set_values("do",20000+Mdo_list[5],Mdo_list[6])  # 向IOA寄存器中写值
                    self.slave.set_values("robot6",93,0)   # 将MODBUS表中的使能清除掉

                config.set_value("BO0",self.master.bo_out)  



                # 3执行机器人的指令
                cmd = self.slave.get_values("robot6",49,2)
                if cmd[0] == 1:
                    P = []
                    motion = self.slave.get_values("robot6",31,18)
                    for i in range(0,14,2):
                        P.append(self.int2float(motion[i],motion[i+1]))
                    self.robot.move(P=P,speed=motion[17],coordinate=motion[14],is_relative=motion[15],mode=motion[16])
                    self.slave.set_values("robot6",49,0)
                
                # 复位等命令
                match cmd[1]:
                    case 0:
                        pass
                    case 1:
                        self.robot.home_1axis(1)
                        self.slave.set_values("robot6",50,0)
                        
                    case 2:
                        self.robot.home_1axis(2)
                        self.slave.set_values("robot6",50,0)
                        
                    case 3:
                        self.robot.home_1axis(3)
                        self.slave.set_values("robot6",50,0)
                        
                    case 4:
                        self.robot.home_1axis(4)
                        self.slave.set_values("robot6",50,0)
                    case 5:
                        self.robot.home_1axis(5)
                        self.slave.set_values("robot6",50,0)

                    case 6:
                        self.robot.home_1axis(6)
                        self.slave.set_values("robot6",50,0)

                    case 7:
                        self.robot.home_1axis(7)
                        self.slave.set_values("robot6",50,0)

                    case 8:
                        self.robot.home()
                        self.slave.set_values("robot6",50,0)

                    case 9:
                        self.robot.home_6axis_in_turn()
                        self.slave.set_values("robot6",50,0)

                    case 10:
                        self.robot.home_7axis()
                        self.slave.set_values("robot6",50,0)

                    case 11:
                        self.robot.unlock_all_axis()
                        self.slave.set_values("robot6",50,0)

                    case 12:
                        self.robot.stop()
                        self.slave.set_values("robot6",50,0)

                    case 13:
                        self.robot.go_to_zero()
                        self.slave.set_values("robot6",50,0)


                #4 执行485模式


            except Exception as e:
                robot_logger.error(e,exc_info=True)



# 485串口，使用需要指定ID
class Slaves_Driver():

    
    def __init__(self,port="/dev/ttyS5",slave_id=1,signal = None):




        ser = Serial(port=port, baudrate=115200, bytesize=8, parity='N', stopbits=1)
        self.signal =signal
        self.robot = RtuServer(ser)
        self.port = port
        self.slave_id =slave_id
        self.robot.set_timeout(3.0)
        self.robot.set_verbose(True)
        self.reg_init()

    # 创建一个寄存器
    def reg_init(self,):
        try:
            self.slave = self.robot.add_slave(self.slave_id)
            self.slave.add_block('order', cst.HOLDING_REGISTERS, 0, 20)  # 创建20个命令寄存器
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1



class master_io():
    def __init__(self):
        self.gobal_do = 0

        self.A0 = 0
        self.A1 = 1
        self.A2 = 2
        self.A3 = 3
        self.A4 = 4
        self.A5 = 5
        self.A6 = 6
        self.A7 = 7

        self.B0 = 8
        self.B1 = 9
        self.B2 = 10
        self.B3 = 11
        self.B4 = 12
        self.B5 = 13
        self.B6 = 14
        self.B7 = 15      

        self.C0 = 16
        self.C1 = 17 
        self.C2 = 18
        self.C3 = 19 
        self.C4 = 20
        self.C5 = 21
        self.C6 = 22
        self.C7 = 23

        self.chip0 = "/dev/gpiochip0"  # chip0
        self.chip1 = "/dev/gpiochip1"  # chip1
        self.chip2 = "/dev/gpiochip2"  # chip2
        self.chip3 = "/dev/gpiochip3"  # chip3
        self.chip4 = "/dev/gpiochip4"  # chip4

        self.BUTTON1 = GPIO(self.chip1, self.A0, "in")  # 拨码按钮   
        self.BUTTON2 = GPIO(self.chip1, self.A1, "in")  # 离线按钮
        self.BUTTON3 = GPIO(self.chip1, self.A4, "in")  # 复位按钮

        self.DI_9 = GPIO(self.chip1, self.B2, "in")  # DI9 端口
        self.DI_8 = GPIO(self.chip1, self.B1, "in")  # DI8 端口

        self.DI_7 = GPIO(self.chip4, self.C3, "in")  # DI7 端口
        self.DI_6 = GPIO(self.chip4, self.C5, "in")  # DI6 端口
        self.DI_5 = GPIO(self.chip4, self.C2, "in")  # DI5 端口
        self.DI_4 = GPIO(self.chip3, self.B4, "in")  # DI4 端口
        self.DI_3 = GPIO(self.chip3, self.C1, "in")  # DI3 端口
        self.DI_2 = GPIO(self.chip3, self.B6, "in")  # DI2 端口
        self.DI_1 = GPIO(self.chip3, self.B5, "in")  # DI1 端口
        self.DI_0 = GPIO(self.chip1, self.B0, "in")  # DI0 端口

        self.Motor_0 = GPIO(self.chip3, self.B2, "out")  # 电机正转
        self.Motor_1 = GPIO(self.chip3, self.B3, "out")  # 电机反转
        self.LED2 = GPIO(self.chip3, self.A6, "out")  # 绿色
        self.LED3 = GPIO(self.chip3, self.A7, "out")  # 蓝色
        # self.LED4 = GPIO(self.chip4, self.C2, "out")  # 红色

        self.DO0 = GPIO(self.chip3, self.B1, "out")  # DO输出1
        self.DO1 = GPIO(self.chip3, self.B0, "out")  # DO输出2
        self.DO2 = GPIO(self.chip4, self.C6, "out")  # DO输出3
        self.DO3 = GPIO(self.chip4, self.C4, "out")  # DO输出4

        self.bo_out =0x00  

    def bo_count(self,BO,bo_num,value):# 定义一个按位计算的函数
        # BO 是一个8位的16进制数
        # bo_bit 是第几个IO
        # value 是DO的值
        # 将DO状态上传IOA服务器

        try:
            bit_num = 0b00000000
            if value ==0:
                match bo_num:
                    case 0 :
                        bit_num = 0b11111110
                    case 1 :
                        bit_num = 0b11111101

                    case 2 :
                        bit_num = 0b11111011
                    case 3 :
                        bit_num = 0b11110111
                    case 4 :
                        bit_num = 0b11101111
                    case 5 :
                        bit_num = 0b11011111
                    case 6 :
                        bit_num = 0b10111111
                    case 7 :
                        bit_num = 0b01111111
                BO  =  BO & bit_num
            
            elif value ==1:
                match bo_num:
                    case 0:
                        bit_num = 0b00000001
                    case 1:
                        bit_num = 0b00000010
                    case 2:
                        bit_num = 0b00000100
                    case 3:
                        bit_num = 0b00001000
                    case 4:
                        bit_num = 0b00010000
                    case 5:
                        bit_num = 0b00100000
                    case 6:
                        bit_num = 0b01000000
                    case 7:
                        bit_num = 0b10000000
                BO  =  BO | bit_num
            
            return BO
                    

        except Exception as e:
            print(e)

    def get_DI(self,num :int =1):
        try:
            value = -1

            match num:
                case 1:
                    value = self.BUTTON1.read()
                case 2:
                    value = self.BUTTON2.read()   
                case 3:
                    value = self.BUTTON3.read() 
                case 4:
                    value = self.DI_0.read() 
                case 5:
                    value = self.DI_1.read() 
                case 6:
                    value = self.DI_2.read() 
                case 7:
                    value = self.DI_3.read() 
                case 8:
                    value = self.DI_4.read() 
                case 9:
                    value = self.DI_5.read() 
                case 10:
                    value = self.DI_6.read() 
                case 11:
                    value = self.DI_7.read() 
                case 12:
                    value = self.DI_8.read() 
                case 13:
                    value = self.DI_9.read() 
              
            return value
        except Exception as e:
            robot_logger.error(e,exc_info=True) 

    # 这里是设置的四个灯的状态资源
    def set_LED(self,num,value=True):
        try:
            match num:
                case 0:
                    self.Motor_0.write(value)
                    if value:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=1)
                    else:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=0)
                case 1:
                    self.Motor_1.write(value)
                case 2:
                    self.LED2.write(value)
                case 3:
                    self.LED3.write(value)
                case 4:
                    pass
                    # self.LED4.write(value)


        except Exception as e:
            robot_logger.error(e,exc_info=True)

    #设置DO只需要调用这个函数
    def set_DO(self,num,value=1):
        try:
            value_bool = True
            if value == 0:
                value_bool =False
            elif value == 1:
                value_bool = True
            match num:
                case 0:
                    self.DO0.write(value_bool)
                case 1:
                    self.DO1.write(value_bool)
                case 2:
                    self.DO2.write(value_bool)
                case 3:
                    self.DO3.write(value_bool)

            self.bo_out = self.bo_count(self.bo_out,num,value=value)
        except Exception as e:
            robot_logger.error(e,exc_info=True)



class robot_driver():
    def __init__(self) -> None:

        self.ser_ = Serial(port="/dev/ttyUSB0",baudrate=115200)
        self.thread_update_ = Thread(target=self.task_,)
        self.thread_update_.start()

        self.ser = Serial(port="/dev/ttyS7",baudrate=115200)
        self.thread_update = Thread(target=self.task,)
        self.thread_update.start()
        self.status_global = None
        self.status_4axisData = {}

        self.default_speed = 2000


        
        ########################数据库存储状态############################
        filename = 'ktr6.db'
        file_dir = "/home/cat/gm/data/"
        # # 使用系统分隔符连接目录和文件
        # file_path = os.sep.join([file_dir, filename])
        # 使用路径模块连接
        file_path = os.path.join(file_dir, filename)
        self.conn = sqlite3.connect(file_path,timeout=10,check_same_thread=False)

        self.robot_type  = 0 # 这里来表示机器人类型。0：代表无机器人，1:代表是六轴，2：代表是四轴

    def float2bytes(self,f):
    #将浮点数打包成32位2进制数据
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]  

    def send_tcp_data_(self,data):
         
        j6  = data["angle"][0]
        j7  = data["angle"][1]
        j8  = data["angle"][2]
        j9  = data["angle"][3]
        print(f"status_4axisData-----send to IOA is {j6},{j7},{j8},{j9}")
        # robotdata_byte6=(self.float2bytes(j6))   # 滑轨数据

        robotdata_byte6=(self.float2bytes(j6))
        robotdata_byte7=(self.float2bytes(j7))
        robotdata_byte8=(self.float2bytes(j8))
        robotdata_byte9=(self.float2bytes(j9))

        config.set_value("j6",robotdata_byte6)
        config.set_value("j7",robotdata_byte7)  ###将MS数据存入字典
        config.set_value("j8",robotdata_byte8)
        config.set_value("j9",robotdata_byte9)
    
    # 要发送到TCP服务的数据
    def send_tcp_data(self,data):
        
        j0  = data["angle"][0]
        j1  = data["angle"][1]
        j2  = data["angle"][2]
        j3  = data["angle"][3]
        j4  = data["angle"][4]
        j5  = data["angle"][5]
        print(f"status_6xisData-----send to IOA is {j0},{j1},{j2},{j3},{j4},{j5}")
       
        # j6  = self.status_4axisData["angle"][0]
        # j7  = self.status_4axisData["angle"][1]
        # j8  = self.status_4axisData["angle"][2]
        # j9  = self.status_4axisData["angle"][3]
        # print(f"status_4axisData-----send to IOA is {j6},{j7},{j8},{j9}")


        # print("6轴数据",j0,j1,j2,j3,j4,j5)  ###（测试用）
        robotdata_byte0=(self.float2bytes(j0))
        robotdata_byte1=(self.float2bytes(j1))
        robotdata_byte2=(self.float2bytes(j2))
        robotdata_byte3=(self.float2bytes(j3))
        robotdata_byte4=(self.float2bytes(j4))
        robotdata_byte5=(self.float2bytes(j5))
        # robotdata_byte6=(self.float2bytes(j6))   # 滑轨数据

        # robotdata_byte6=(self.float2bytes(j6))
        # robotdata_byte7=(self.float2bytes(j7))
        # robotdata_byte8=(self.float2bytes(j8))
        # robotdata_byte9=(self.float2bytes(j9))

        config.set_value("j0",robotdata_byte0)  ###将MS数据存入字典
        config.set_value("j1",robotdata_byte1)
        config.set_value("j2",robotdata_byte2)
        config.set_value("j3",robotdata_byte3)
        config.set_value("j4",robotdata_byte4)
        config.set_value("j5",robotdata_byte5)

        # config.set_value("j6",robotdata_byte6)
        # config.set_value("j7",robotdata_byte7)  ###将MS数据存入字典
        # config.set_value("j8",robotdata_byte8)
        # config.set_value("j9",robotdata_byte9)

        # config.set_value("FO[1]",value=robotdata_byte6)
        
    def set_robot_state(self,data):
        try :
            
            c = self.conn.cursor()
            state = data
            en = c.execute("SELECT EN from Robot_state")
            data_en = en.fetchall()

            # 检测是否有G代码要执行
            if data_en[0][0] == 1:
                gcode = c.execute("SELECT GCODE from Robot_state")
                data_gcode = gcode.fetchall()
                self.send_msg(data_gcode[0][0])
                c.execute("UPDATE Robot_state set EN = 0 where ID=1")
                # self.conn.commit() 

            # 将机器人状态更新到数据库里。
            if state=="Idle":
                c.execute("UPDATE Robot_state set STATE = 'Idle' where ID=1")
            elif state=="Alarm":

                c.execute("UPDATE Robot_state set STATE = 'Alarm' where ID=1")
            elif state=="Home":

                c.execute("UPDATE Robot_state set STATE = 'Home' where ID=1")
            elif state=="Run":

                c.execute("UPDATE Robot_state set STATE = 'Run' where ID=1")

            self.conn.commit() 
        except Exception as e:
            print(e)
    
    def send_msg(self,message, wait_ok=False, wait_idle=True , terminator=os.linesep):
        try:
            # 自动添加换行符
            if not message.endswith(terminator):
                message += terminator
            # 串口发送数据，编码为utf-8
            self.ser.write(message.encode('utf-8'))
            return True
        except Exception as e:
            # 日志写入串口设备写入异常
            
            robot_logger.error(e,exc_info=True)
            return False

    def send_msg_(self,message, wait_ok=False, wait_idle=True , terminator=os.linesep):
        try:
            # 自动添加换行符
            if not message.endswith(terminator):
                message += terminator
            # 串口发送数据，编码为utf-8
            self.ser_.write(message.encode('utf-8'))
            return True
        except Exception as e:
            # 日志写入串口设备写入异常
            
            robot_logger.error(e,exc_info=True)
            return False

    def robot_type_update(self,):
        try:
            gcode = f"$v"
            self.send_msg(gcode)
            time.sleep(0.1) 
            data = self.ser.read_all() # 从串口读取数据
            recive = str(data,"UTF-8")
            print("receive:"+recive)
            if "Mirobot" in recive:
                self.robot_type =1
            elif "E4" in recive:
                self.robot_type =2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robot_type_update_(self,):
        try:
            gcode = f"$v"
            self.send_msg_(gcode)
            time.sleep(0.1) 
            data = self.ser_.read_all() # 从串口读取数据
            recive = str(data,"UTF-8")
            print("receive:"+recive)
            if "Mirobot" in recive:
                self.robot_type_ =1
            elif "E4" in recive:
                self.robot_type_ =2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def task_(self,):
        type_flag_count = 0   
        while True:
            try:
                gcode = f"?"
                self.send_msg_(gcode)
                time.sleep(2)     
                data = self.ser_.read_all() # 从串口读取数据
                # data_ = data.decode('utf-8')
                print(f"----2024-3-15-----4axies read data is {data}------")
                # print(data) # 将读取的数据转换为字符串并回显

                # 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    if self.robot_type == 0:  # 进来第一次询问
                        self.robot_type_update_()
                else:
                    type_flag_count +=1
                    print(type_flag_count)
                    if type_flag_count == 10:
                        self.robot_type = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    # print(f"----2024-3-15----Kata_status2json_4axie data is {data}------")
                    self.Kata_status2json_4axies(data)
                    # print(self.status_global)
                    # self.set_robot_state(self.status_global["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def task(self,):

        type_flag_count = 0   
        while True:
            try:
                # time.sleep(0.01)

                gcode = f"?"
                self.send_msg(gcode)
                time.sleep(0.1)     
                data = self.ser.read_all() # 从串口读取数据
                # print(data) # 将读取的数据转换为字符串并回显

                # 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    if self.robot_type == 0:  # 进来第一次询问
                        self.robot_type_update()
 
                else:
                    type_flag_count +=1
                    print(type_flag_count)
                    if type_flag_count == 10:
                        self.robot_type = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    self.Kata_status2json(data)
                    # print(self.status_global)
                    self.set_robot_state(self.status_global["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def Kata_status2json_4axies(self,data):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            if status_date !=None:
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type

                    }
            if status != None:
                self.status_4axisData = status
                print(f"status_4axisData data is {self.status_4axisData}")
                # self.status_global = status
                # print(self.status_global)
                self.send_tcp_data_(status)
            return self.status_4axisData

        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def Kata_status2json(self,data):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            if status_date !=None:
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type

                    }
            if status != None:
                self.status_global = status
                # print(self.status_global)
                self.send_tcp_data(status)
            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    ####复位
    def home(self, has_slider=False):
        '''机械臂Homing'''
        if has_slider:
            return self.home_7axis()
        else:
            print("home")
            return self.home_6axis()

    def home_1axis(self, axis_id):
        '''单轴Homing'''
        if not isinstance(axis_id, int) or not (axis_id >= 1 and axis_id <= 7):
            return False
        msg = f'$h{axis_id}'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis(self):
        '''六轴Homing'''
        msg = f'$h'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis_in_turn(self):
        '''六轴Homing, 各关节依次Homing'''
        msg = f'$hh'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
    
    def stop(self):
        '''6轴暂停'''
        msg = f'!'
        self.send_msg(msg, wait_ok=False, wait_idle=False)
        time.sleep(0.5)
        msg = f'%'
        return self.send_msg(msg, wait_ok=False, wait_idle=False)

    def home_7axis(self):
        '''七轴Homing(本体 + 滑台)'''
        msg = f'$h0'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
        
    def unlock_all_axis(self):
        '''解锁各轴锁定状态'''
        msg = 'M50'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)
        
    def go_to_zero(self):
        '''回零-运动到名义上的各轴零点'''
        msg = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def set_speed(self, speed):
        '''设置转速'''
        # 转换为整数
        speed = int(speed)
        # 检查数值范围是否合法
        if speed <= 0 or speed > 6000:
            robot_logger.error(f"Illegal movement speed {speed}")
            return False
        # 发送指令
        msg = f'F{speed}'
        return self.send_msg(msg, wait_ok=None, wait_idle=None)

    def set_hard_limit(self, enable):
        '''
        开启硬件限位
        '''
        msg = f'$21={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def set_soft_limit(self, enable):
        '''开启软限位
        注: 请谨慎使用
        '''
        msg = f'$20={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def format_float_value(self, value):
        if value is None:
            return value
        if isinstance(value, float):
            # 精确到小数点后两位数
            return round(value , 2)
        else:
            return value
    
    def generate_args_string(self, instruction, pairings):
        '''生成参数字符'''
        args = [f'{arg_key}{self.format_float_value(value)}' for arg_key, value in pairings.items() if value is not None]

        return ' '.join([instruction] + args)
        
    def move(self, P, speed = None ,coordinate= 1, is_relative=0,mode =0, wait_ok=None):

        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        instruction = 'M20 G90 G1'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        m1 = "M20"             # 世界坐标系
        pairings = {'X': P[0], 'Y': P[1], 'Z': P[2], 'A': P[3], 'B': P[4], 'C': P[5], 'D':P[6], 'F': speed}
        if coordinate ==0:
            m1 = "M21"      # 轴角坐标系
        m2 = "G90"
        if is_relative:
            m2 = 'G91'       
        m3 ="G00"
        if mode == 1:
            m3 = "G01"

        instruction = f"{m1} {m2} {m3} "
        
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def circular_interpolation(self, ex, ey, radius, is_cw=True, speed=None, wait_ok=None):
        '''圆弧插补
        在XY平面上, 从当前点运动到相对坐标(ex, ey).半径为radius
        `is_cw`决定圆弧是顺时针还是逆时针.
        '''
        # 判断是否合法
        distance = math.sqrt(ex**2 + ey**2)
        if distance > (radius * 2):
            self.logger.error(f'circular interpolation error, target posi is too far')
            return False

        instruction = None
        if is_cw:
            instruction = 'M20 G91 G02'
        else:
            instruction = 'M20 G91 G03'
        
        pairings = {'X': ex, 'Y': ey, 'R': radius, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def set_door_lift_distance(self, lift_distance):
        '''设置门式轨迹规划抬起的高度'''
        msg = f"$49={lift_distance}"
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def door_interpolation(self, x=None, y=None, z=None, a=None, b=None, c=None, speed=None, is_relative=False, wait_ok=None):
        '''门式插补'''
        instruction = 'M20 G90 G05'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        if is_relative:
            instruction = 'M20 G91 G05'
        
        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)


      
###tcp向IOA发送数据包
class TcpServer():

    
    def __init__(self) -> None:
        self.tcpServer()
    def tcpServer(self):
        self.tcp_server_socket=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        self.tcp_server_socket.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,True)
        address2=config.get_value('address2')

        config.set_value(name="BO0",value=0)
        config.set_value(name="BO1",value=0) #初始化16个DO 
        speed_value=(self.float2bytes(75.0))
        config.set_value("FO[0]",value=speed_value)
        f01=(self.float2bytes(75.0))
        config.set_value("FO[1]",value=f01)

        self.tcp_server_socket.bind(address2)
        self.tcp_server_socket.listen(10)

    def float2bytes(self,f):
    #将浮点数打包成32位2进制数据
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]      

    def checksum(self,Data,lenth):
        checksum_value=0
        data=list()
        for i in range(lenth-2):   #######从Data[2]开始，相应总长度减2
            data.append(Data[i+2])    
        for i in range(len(data)):
            checksum_value=((checksum_value+data[i]))%0xFFFF
        checksum_value_low=(checksum_value&0xFF)
        return checksum_value_low
    def calculate_recvdata(self,recv_value,lenth):
        
        data=list()                       
        for i in range(0,lenth):
            data.append(recv_value[i])
        print('接收到IOA数据')
        config.set_value("BI0",data[12])    ###16位BI数据中低8位BI0----BI7                    
        config.set_value("BI1",data[13])    ###16位BI数据中高8位BI8----BI15
        ###4个一组为一个FI，共4个FI,data[17]-data[32]
        tf0=(data[17:21])
        tf1=(data[21:25])
        tf2=(data[25:29])
        tf3=(data[29:33])
        config.set_value("FI[0]",value=tf0)
        config.set_value("FI[1]",value=tf1) ##FI.append(data[17+4*i:21+4*i])
        config.set_value("FI[2]",value=tf2)
        config.set_value("FI[3]",value=tf3)  
  
    '''    ####小端字节序按位读取BI口数据
                BI0=(data[12]&0x1)
                BI1=(data[12]&0x2)>>1
                BI2=(data[12]&0x4)>>2
                BI3=(data[12]&0x8)>>3
                BI4=(data[12]&0x10)>>4
                BI5=(data[12]&0x20)>>5
                BI6=(data[12]&0x40)>>6
                BI7=(data[12]&0x80)>>7
                
                
                BI16=(data[13]&0x1)
                BI17=(data[13]&0x2)>>1
                BI18=(data[13]&0x4)>>2
                BI19=(data[13]&0x8)>>3
                BI20=(data[13]&0x10)>>4
                BI21=(data[13]&0x20)>>5
                BI22=(data[13]&0x40)>>6
                BI23=(data[13]&0x80)>>7
                BI.append(BI0,BI1)   '''
    def set_sendData2IOA(self):

        '''
        如果要使用数字孪生，需要根据实际来设置
        id :如果有两个控制器，另外一个要设置成2
        机器人型号：四轴是39，六轴是41
        这里需要两个参数：设置id、机器人型号
        '''
        
        data=list()
        ###帧头###
        data.extend([0xff,0xaa])        #00,01
        ###协议号###
        data.append(0x01)               #02
        ###帧长度###53---57
        data.extend([0x63,0x00])        #03,04
        ###机器人型号###
        data.extend([0x41,0x00])        #05,06
        ###设备id号;外设类型数量###
        data.extend([0x01,0x05])        #07,08
        ###子帧（1）16个BI数据，来自IOA###
        data.append(0x01)               #09
        data.extend([0x02,0x00])        #10,11
        data.extend([config.get_value('BI0'),config.get_value('BI1')])  #12,13
        ###子帧（2）16个BO数据来自ABB ### 
        data.append(0x02)                           #14
        data.extend([0x02,0x00])                      #15,16       
        data.append(config.get_value("BO0"))        #17
        # data.append(0xff)  
        data.append(config.get_value("BO1"))        #18
        ###子帧（3）6  + 4个MS，float数据，来自ABB  0x18---0x28  ###
        data.append(0x03)                           #19
        data.extend([0x28,0x00])                    #20,21
        data.extend(config.get_value("j0"))         #22-25
        data.extend(config.get_value("j1"))         #26-29
        data.extend(config.get_value("j2"))         #30-33
        data.extend(config.get_value("j3"))         #34-37
        data.extend(config.get_value("j4"))         #38-41
        data.extend(config.get_value("j5"))         #42-45

        data.extend(config.get_value("j6")) 
        data.extend(config.get_value("j7"))         #34-37
        data.extend(config.get_value("j8"))         #38-41
        data.extend(config.get_value("j9"))         #42-45  
                #30-33     
        ###子帧（4）4个FI，float数据，来自IOA ###
        data.append(0x84)                           #46
        data.extend([0x10,0x00])                    #47,48
        data.extend(config.get_value("FI[0]"))      #49-52
        data.extend(config.get_value("FI[1]"))      #53-56
        data.extend(config.get_value("FI[2]"))      #57-60
        data.extend(config.get_value("FI[3]"))      #61-64
        ###子帧（5） 4个FO数据，float，来自ABB ###
        data.append(0x85)                           #65
        data.extend([0x10,0x00])                    #66,67
        #Fo数据来自ABB示教器，暂定为0后续增加相关函数#68-83
        data.extend(config.get_value("FO[0]"))      #FO 0
        data.extend(config.get_value("FO[1]"))      #FO 0
        data_FO=[0x00]*8

        data.extend(data_FO)
        ###添加校验和###
        checksum_value=self.checksum(data,100)       #84
        data.append(checksum_value)
        ###添加帧尾###
        data.extend([0xff,0x55])                    #85,86+16
        # with open('my_list.json', 'w') as f:  
        #     json.dump(data, f)

        print(f"2024-3-15-length is --{len(data)} ----data is send to IOA IS {data}----------2024-3-15")
        return data
    
    def run(self):
        while True:
            
            client_socket,client_Addr=self.tcp_server_socket.accept()
            print("from:",client_Addr)
            time.sleep(0.1)
            while True:
                try:
                    recv_data=client_socket.recv(1024)
                    if len(recv_data)<36:  ##############处理接收数据
                        client_socket,client_Addr=self.tcp_server_socket.accept()
                        pass
                    else:
                        for i in range(0,len(recv_data)):
                            if (recv_data[i]==0xff and recv_data[i+1]==0xaa):
                                recv_data=recv_data[i:]
                                break                  
                        if (recv_data[0]==0xff and recv_data[1]==0xaa):
                            if (recv_data[8]==0x02):
                                if (recv_data[9]==0x01 and recv_data[10]==0x02 and recv_data[14]==0x84 and recv_data[15]==0x10):
                                    self.calculate_recvdata(recv_data,36)
                                    senddata=self.set_sendData2IOA()
                                    send_data=bytes(senddata)
                                    print(f"--*******length is --{len(send_data)}**********--send_data is {send_data}******************")
                                    client_socket.send(send_data)                                                               
                                else:
                                    print("congfig参数不对")
                            else:
                                print("外设数目不为2")                            
                        else:
                            print('报头校验不通过')
                            
                except Exception as e:
                    robot_logger.error(e,exc_info=True)
                    break

            # client_socket.close()
                # break
        self.tcp_server_socket.close()


a = Controler_Server()
a.dorun()

# b=  TcpServer()
# b.run()

